/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

// File: serialTest.cpp
// Author: Cory Dixon
// Date: 2/27/01
//
// Program to test the serial port both in Unix
// and VxWorks
//

#include <unistd.h>
#include <iostream.h>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <sys/types.h>
#include <fcntl.h>

#include "String.h"
#include "serial.h"
#include "timeLib.h"
#include "command.h"
#include "queue.h"
#include "type_defs.h"

// functions
void clientMode(char *dev, char *baud);
void hostMode(char *dev, char *baud);
void parseClientCommand(Command &cmd);
void parseHostCommand(Command &cmd);

#define MAXLINE 256

// global data
SerialPort *serialHost;
SerialPort *serialClient;

//
// Global Data
//
TimeStamp tsTx;
TimeStamp tsRx;
long long totalTime = 0;
unsigned long int minTime   = 999999999;
unsigned long int maxTime   = 0;
bool hostConnected = false;
bool clientConnected = false;

#define NUM_LOOP	1024
Queue timeQueue(NUM_LOOP);

//----------------------------------------------------------
// Act on received commands
//----------------------------------------------------------
void parseClientCommand(Command &cmd)
{
	if( !cmd.isValid() ) {
		cout << "<!!! parseCommand - invalid checksum !!!>\n";
		cmd.printCmd();

		return;
	}	  

	switch( cmd.cmd() ) {
	// check if was host replying to connection request
	case CONNECTING:
		cout << "<--- Connected to host --->" << endl;
		clientConnected = true;
		break;

	// check if exiting
	case DISCONNECTING:
		cout << "<--- Reqeust from host to disconnect --->" << endl;
		clientConnected = false;
		break;

	// host returned the time command
	case TIME_LOOP:
	{
		long deltaT;
		register float msec;

		// calculate the time diff
		tsRx.stamp();
		deltaT = tsRx - tsTx;
		maxTime = (deltaT > maxTime ? deltaT : maxTime);
		minTime = (deltaT < minTime ? deltaT : minTime);
		totalTime += deltaT;
		msec = deltaT / 1000.0;
		timeQueue.Push( (QueueType *)&(msec) ); // save as milli-sec
	}
		break;

	default:
		cout << "<!!! Unknown command " << cmd.cmd() << " !!!>" << endl;
		break;
	}

}

void parseHostCommand(Command &cmd)
{
	if( !cmd.isValid() ) {
		cout << "<!!! parseCommand - invalid checksum !!!>\n";
		cmd.printCmd();

		return;
	}	  

	switch( cmd.cmd() ) {
	// check if connecting
	case CONNECTING:
	{
		cout << "<--- Request to connect --->" << endl << flush;

		uchar8 cmdBuf[] = {CONNECTING, 0, 0, 0};
		Command send(cmdBuf);
		send.setCheckSum();

		if( serialHost->write(send) != send.size() )
			cout << "!!! Could not connect client !!!" <<endl; 
		else {
			cout << "Client connected" << endl;
			hostConnected = true;
		}
	}
		break;

	// check if exiting
	case DISCONNECTING:
		cout << "<--- Reqeust to disconnect --->" << endl;
		break;

	case TIME_LOOP:
		cout << "<--- Time-Loop --->" << endl << flush;

		// simply send it back ASAP
		if( serialHost->write(cmd) != cmd.size() ){
			cout << "<!!! Could not send back time-loop!!!>" <<endl; 
			perror("serial->write" );
			return;
		}
		break;

	default:
		cout << "<!!! Unknown command " << cmd.cmd() << " !!!>" << endl;
		break;
	}
}

//----------------------------------------------------------
// Run like a Client
//----------------------------------------------------------
void clientMode(char *dev, char *baud)
{
	uchar8 *rxBuf = new uchar8[ CMD_MAX_SIZE ];
	int rxNum = 0;
	CommandQueue serialQueue;		// parse incoming serialet stream
	SerialPort::SerialWait serialWait;	// return value for wait

	serialClient = new SerialPort(dev,baud);
	assert(serialClient != NULL);
	if( serialClient->init() != SerialPort::Connected ){
		printf("Could not open serial port\n");
	}
	serialClient->setNonBlocking();

	// time command
	uchar8 cmdBuf[CMD_MAX_SIZE] = { 127 };
	cmdBuf[0] = TIME_LOOP;
	cmdBuf[1] = CMD_MAX_SIZE - CMD_PACKET_SIZE;
	Command time(cmdBuf);
	time.setCheckSum();

	// connect command
	uchar8 cntBuf[] = {CONNECTING, 0, 0, 0};
	Command cntCmd(cntBuf);
	cntCmd.setCheckSum();

	int i=0;
	while( i < NUM_LOOP) 
	{
		// try to connect to host
		if( clientConnected == false && serialClient->getStatus() == SerialPort::Connected ) {
			cout << "Trying to connect to host" << endl;

			// don't let output buffer build up
			// serialClient->flushIO();

			// send to host
			if( serialClient->write(cntCmd) != cntCmd.size() ){
				cout << "!!! Could not send client connecting !!!" <<endl; 
				perror ("serialClient->write");
				break;
			}
		} else if( clientConnected == true && serialClient->getStatus() == SerialPort::Connected ) { 

			cout << "sending time-loop" << endl;
			tsTx.stamp();
			if( serialClient->write(time) != time.size() ){
				cout << "<!!! Could not send time-loop!!!>" <<endl;
				perror ("serialClient->write");
				return;
			}

			i++;
		} else {
			cout << "Serial port not connected" << endl;
			break;
		}

		// wait 1000 milli-secs for reply
		serialWait = serialClient->wait(1000); // wait

		if( serialWait == SerialPort::WAIT_TIMEOUT) {
			cout << "serialClient->wait timedout" << endl;

			// don't let output buffer build up
			// serialClient->flushIO();
		}
		else if(serialWait == SerialPort::WAIT_INT)
			cout << "serialClient->wait interrupted" << endl;
		else if(serialWait == SerialPort::WAIT_ERROR)
			cout << "serialClient->wait error" << endl;
		else if( serialWait == SerialPort::WAIT_DATA) 
		{
			// get the command from host
			rxNum = serialClient->read((char *)rxBuf, CMD_MAX_SIZE);
			cout << "clientMode() - read " << rxNum << endl;

			if( rxNum > 0) {
				serialQueue.push(rxBuf,rxNum);
				while( serialQueue.length() >= CMD_PACKET_SIZE) {

					// check the cmd and numParams bytes
					if( serialQueue.isDefined() ) {

						// start is good, do we have enough bytes
						if( !serialQueue.needMoreBytes() ) {
							if( serialQueue.isValid() ){
								Command rxCmd = serialQueue.popCommand();
								parseClientCommand(rxCmd);
							} else {
								cout << "-- Dropping byte for bad commad --" << endl
									<< (int)serialQueue.popByte() << endl;
							}
						} else
							break;
					} else {
						cout << "-- Dropping byte for bad header --" << endl
							<< (int)serialQueue.popByte() << endl;
					}
				}


			} else {
				if( rxNum == 0)
					cout << "Read 0 bytes" << endl;
				else if( errno != EWOULDBLOCK || errno != EINTR)
					perror("clientMode() - bad read from host");

				serialClient->close();
			}
		} else if( errno != EINTR ) {
			perror("clientMode() - serialClient->wait()");
		}
	} // end of for()

	cout << "Average time = " << totalTime / 1000.0 / (float)NUM_LOOP << "ms " << endl;
	cout << "minTime time = " << (float)(minTime / 1000.0)   << "ms " << endl;
	cout << "maxTime time = " << (float)(maxTime / 1000.0)   << "ms " << endl;

	// save data to file
	timeQueue.saveToFile("serialClient_time.dat" ); // save as milli-sec

	delete serialClient;
}

//----------------------------------------------------------
// Run like a host
//----------------------------------------------------------
void hostMode(char *dev, char *baud)
{
	SerialPort::SerialWait serialWait;
	Command cmd;
	uchar8 *rxBuf = new uchar8[ CMD_MAX_SIZE ];
	CommandQueue serialQueue; // parse incoming serialet stream
	int num;

	// open serial port
	serialHost = new SerialPort(dev, baud);
	assert(serialHost != NULL);

	if( serialHost->init() != SerialPort::Connected ){
		printf("Could not open serial port\n");
	}

	serialHost->setNonBlocking();

	while(1)
	{
		// wait for input, timeout at 1000 milli-sec
		serialWait = serialHost->wait(1000);

		if( serialWait == SerialPort::WAIT_TIMEOUT)
			cout << "serialHost->wait timedout" << endl;
		else if(serialWait == SerialPort::WAIT_INT)
			cout << "serialHost->wait interrupted" << endl;
		else if(serialWait == SerialPort::WAIT_ERROR)
			cout << "serialHost->wait error" << endl;
		else if( serialWait == SerialPort::WAIT_DATA) 
		{
			cout << "Reading data" << endl;
			// get the command from client
			num = serialHost->read((char *)rxBuf, CMD_MAX_SIZE);

			if( num > 0) {
				serialQueue.push(rxBuf,num);
				while( serialQueue.length() >= CMD_PACKET_SIZE) {

					// check the cmd and numParams bytes
					if( serialQueue.isDefined() ) {

						// start is good, do we have enough bytes
						if( !serialQueue.needMoreBytes() ) {
							if( serialQueue.isValid() ){
								Command rxCmd = serialQueue.popCommand();
								parseHostCommand(rxCmd);
							} else {
								cout << "-- Dropping byte for bad csum --" << endl
									<< (int)serialQueue.popByte() << endl;
							}
						} else
							break;
					} else {
						cout << "-- Dropping byte for bad header --" << endl
							<< (int)serialQueue.popByte() << endl;
					}
				}

			} else {
				if( num == 0)
					cout << "Read 0 bytes" << endl;
				else if( errno != EWOULDBLOCK || errno != EINTR)
					perror("serverMode() - bad read from client");
			}
		}
	} // end of while

	delete serialHost;
}
